/********************************** (C) COPYRIGHT *******************************
* File Name          : foc.c
* Author             : nx666
* Version            : V1.0.0
* Date               : 2023/02/27
* Description        : foc主要接口这里实现接口.
*******************************************************************************/
/***********************************头文件区域***********************************/
#include "IQmathLib.h"
#include "driver.h"
#include "common_type.h"
#include "common_math.h"
#include "module_config.h"
#include "pll.h"
#include "projdefs.h"
#include "smo.h"
#include "stdio.h"
#include "string.h"
#include "FreeRTOS.h"
#include "semphr.h"
#include "foc.h"
#include <math.h>
#include "interact.h"
#include "modbus.h"


/***********************************类型定义*********************************/
typedef struct 
{
    int id;   //基本向量id 0,1,2,3,4,5,6,7
    int ts;   //作用时间，可直接用于设置pwm占空比
} baseVectorTimes;

/***********************************宏定义区***********************************/
#define _IQ8_SQRT3_DIV_3   (148)
#define _IQ8_2_MPY_SQRT3_DIV_3   (296)

/******************************** *全局变量区**********************************/
// static uInt16 frameTable[6][3] = {
//     {PWM_DEFAULT_ARR/4*3,PWM_DEFAULT_ARR/4*2,PWM_DEFAULT_ARR/4*1},
//     {PWM_DEFAULT_ARR/4*2,PWM_DEFAULT_ARR/4*3,PWM_DEFAULT_ARR/4*1},
//     {PWM_DEFAULT_ARR/4*1,PWM_DEFAULT_ARR/4*3,PWM_DEFAULT_ARR/4*2},
//     {PWM_DEFAULT_ARR/4*1,PWM_DEFAULT_ARR/4*2,PWM_DEFAULT_ARR/4*3},
//     {PWM_DEFAULT_ARR/4*2,PWM_DEFAULT_ARR/4*1,PWM_DEFAULT_ARR/4*3},
//     {PWM_DEFAULT_ARR/4*3,PWM_DEFAULT_ARR/4*1,PWM_DEFAULT_ARR/4*2},
// };
static foc_ *pgFoc = NULL;

/*********************************************************************
 * @fn      createClarkeMat
 *
 * @brief   构建clarke变换矩阵
 *
 * @return  0,成功  -1，失败
 *          
 */
static Int createClarkeMat(clarkeMat_ * pMat)
{
    if (!pMat)
    {
        return -1;
    }
    pMat->mat00 = _IQ8(1);
    pMat->mat01 = _IQ8(-0.5);
    pMat->mat02 = _IQ8(-0.5);
    pMat->mat10 = _IQ8(0);
    pMat->mat11 = _IQ8div(_IQ8sqrt(_IQ8(3)),_IQ8(2));
    pMat->mat12 = -pMat->mat11;

    pMat->mat00 = _IQ8mpy(pMat->mat00, _IQ8(CLARKE_CONVERT_N));
    pMat->mat01 = _IQ8mpy(pMat->mat01, _IQ8(CLARKE_CONVERT_N));
    pMat->mat02 = _IQ8mpy(pMat->mat02, _IQ8(CLARKE_CONVERT_N));

    pMat->mat10 = _IQ8mpy(pMat->mat10, _IQ8(CLARKE_CONVERT_N));
    pMat->mat11 = _IQ8mpy(pMat->mat11, _IQ8(CLARKE_CONVERT_N));
    pMat->mat12 = _IQ8mpy(pMat->mat12, _IQ8(CLARKE_CONVERT_N));


    return 0;
}

/*********************************************************************
 * @fn      foc_IaIbIcToIalphaIbeta
 *
 * @brief   三相电流clarke变换
 *              pFoc
 *
 * @return  void
 *          
 */
static void foc_IaIbIcToIalphaIbeta(struct foc *pFoc)
{
    focParam_        *pParam = &pFoc->param;
    sampling_        *pSamp = pFoc->sampling;
    clarkeMat_       *pMat = &pFoc->clarkeMat;

#if 0
    pParam->Ialpha = _IQ8mpy(pMat->mat00, pSamp->Ia) + _IQ8mpy(pMat->mat01, pSamp->Ib) + _IQ8mpy(pMat->mat02, pSamp->Ic);
    pParam->Ibeta = _IQ8mpy(pMat->mat10, pSamp->Ia) + _IQ8mpy(pMat->mat11, pSamp->Ib) + _IQ8mpy(pMat->mat12, pSamp->Ic);
#else
    //两电阻采样（Ic = -Ia -Ib），等赋值clarke变化，可优化成如下代码：
    pParam->Ialpha = pSamp->Ia;
    pParam->Ibeta = _IQ8mpy(_IQ8_SQRT3_DIV_3, pSamp->Ia) + _IQ8mpy(_IQ8_2_MPY_SQRT3_DIV_3, pSamp->Ib);
#endif
    // printMat[0] = (float)pParam->Ialpha;
    // printMat[1] = (float)pParam->Ibeta;
}

/*********************************************************************
 * @fn      foc_IalphaIbetaToIdIq
 *
 * @brief   采样电流的park变换
 *              pFoc
 *
 * @return  void
 *          
 */
static void foc_IalphaIbetaToIdIq(struct foc *pFoc)
{
    focParam_        *pParam = &pFoc->param;
    _iq      Theta = 0;
    
    Theta = pParam->Theta;
#if 1
    pParam->Id = _IQ8mpy(_IQ8cos(Theta), pParam->Ialpha) + _IQ8mpy(_IQ8sin(Theta), pParam->Ibeta);
    pParam->Iq = _IQ8mpy(-_IQ8sin(Theta), pParam->Ialpha) + _IQ8mpy(_IQ8cos(Theta), pParam->Ibeta);
#else
    pParam->Id = _IQ8mpy(_IQ8cos(Theta), pParam->Ibeta) + _IQ8mpy(_IQ8sin(Theta), pParam->Ialpha);
    pParam->Iq = _IQ8mpy(-_IQ8sin(Theta), pParam->Ibeta) + _IQ8mpy(_IQ8cos(Theta), pParam->Ialpha);
#endif
}

/*********************************************************************
 * @fn      foc_reversePark
 *
 * @brief   实现反park变换，
 *          Vd_w,Vq_w输入期望的[Vd,Vd]旋转坐标系电压
 *          theta 输入当前电角度
 * @return  FUNC_SOK，成功  FUNC_FAIL，失败
 *          
 */
static int foc_reversePark(_iq8 Vd_w, _iq8 Vq_w, _iq8 theta, foc_ *pFoc)
{
    _iq8 Valpha, Vbeta;

    //Valpha = cos(theta)*Vd_w - sin(theta)*Vq_w
    Valpha = _IQ8mpy(_IQ8cos(theta),Vd_w) - _IQ8mpy(_IQ8sin(theta),Vq_w);
    //Vbeta = sin(theta)*vd_w + cos(theta)*vq_w
    Vbeta = _IQ8mpy(_IQ8sin(theta),Vd_w) + _IQ8mpy(_IQ8cos(theta),Vq_w);
    
    pFoc->clac_Ualpha = Valpha;
    pFoc->clac_Ubeta = Vbeta;

    return 0;
}

/*********************************************************************
 * @fn      foc_reverseClarke
 *
 * @brief   实现反clarke变换，
 *          Valpha,Vbeta输入[Valpha,Vbeta]静态坐标系的电压
 *   
 * @return  FUNC_SOK，成功  FUNC_FAIL，失败
 *          
 */
static int foc_reverseClarke(_iq8 Valpha,_iq8 Vbeta,foc_ *pFoc)
{
#if CHOSE_SECTOR
    _iq8 Ua,Ub,Uc;
    _iq8 N = _IQ8((float)2/3);
    _iq8 k1 = _IQ8(0.5); //1/2
    _iq8 k2 =_IQ8mpy(_IQ8sqrt(_IQ8(3)),_IQ8(0.5));  //sqrt(3)/2

    //Ua=N*Valpha  -- > U1
    Ua = _IQ8mpy(N, Vbeta);
    //Ub=N*(-Valpha/2 + sqrt(3)/2*Vbeta) -- > U2
    Ub = _IQ8mpy(N,-_IQ8mpy(k1,Vbeta) + _IQ8mpy(k2,Valpha));
    //Uc=N*(-Valpha/2 - sqrt(3)/2*Vbeta) -- > U3
    Uc = _IQ8mpy(N,-_IQ8mpy(k1,Vbeta) - _IQ8mpy(k2,Valpha));

    pFoc->calc_Ua = Ua;
    pFoc->calc_Ub = Ub;
    pFoc->calc_Uc = Uc;
#else
    _iq8 Ua,Ub,Uc;
    static _iq8 k1 = _IQ8(0.5); //1/2
    static _iq8 k2 = 221;       //_IQ8(sqrt(3)/2)

    //clarke反变换
    //Ua=Valpha  -- > U1
    Ua = Vbeta;
    //Ub=(-Valpha/2 + sqrt(3)/2*Vbeta) -- > U2
    Ub = -_IQ8mpy(k1,Vbeta) + _IQ8mpy(k2,Valpha);
    //Uc=(-Valpha/2 - sqrt(3)/2*Vbeta) -- > U3
    Uc = -_IQ8mpy(k1,Vbeta) - _IQ8mpy(k2,Valpha);

    pFoc->calc_Ua = Ua;
    pFoc->calc_Ub = Ub;
    pFoc->calc_Uc = Uc;

#endif

    return 0;
}



#if CHOSE_SECTOR
/*********************************************************************
 * @fn      foc_calcSector
 *
 * @brief   计算当前所在扇区，
 *          Ua,Ub,Uc输入的三相电压
 *       
 * @return  FUNC_SOK，成功  FUNC_FAIL，失败
 *          
 */
static Int foc_calcSector(_iq8 Ua,_iq8 Ub,_iq8 Uc,foc_ *pFoc)
{
    int A,B,C,N;

    if (Ua > 0)
    {
        A = 1;
    }
    else {
        A = 0;
    }

    if (Ub > 0)
    {
        B = 1;
    }
    else {
        B = 0;
    }

    if (Uc > 0)
    {
        C = 1;
    }
    else {
        C = 0;
    }

    N = 4 * C + 2* B + A;

    switch(N)
    {
        case 1:
        {
            pFoc->calc_sector = 2;
            break;
        }
        case 2:
        {
            pFoc->calc_sector = 6;
            break;
        }
        case 3:
        {
            pFoc->calc_sector = 1;
            break;
        }
        case 4:
        {
            pFoc->calc_sector = 4;
            break;
        }
        case 5:
        {
            pFoc->calc_sector = 3;
            break;
        }
        case 6:
        {
            pFoc->calc_sector = 5;
            break;
        }
        default:
        pFoc->calc_sector = 1;
    }

    return 0;
}

/*********************************************************************
 * @fn      foc_svpwm
 *
 * @brief   实现svpwm，
 *          Ua,Ub,Uc输入的三相电压
 *          sector 当前扇区
 * @return  FUNC_SOK，成功  FUNC_FAIL，失败
 *          
 */
static Int foc_svpwm(_iq8 Ua,_iq8 Ub,_iq8 Uc,int sector,foc_ *pFoc)
{
    //计算基本向量作用时间
    baseVectorTimes ts[3];

    _iq8 K = _IQ8div(_IQ8mpy(_IQ8sqrt(_IQ8(3)),_IQ8(PWM_DEFAULT_ARR)),_IQ8(MOTOR_UDC));   //k=sqrt(3)*Ts/Udc
    _iq8 U1 = Ua ; // U1=Ubeta
    _iq8 U2 = Ub ; // U2=sqrt(3)/2*Ualpha - Ubeta/2
    _iq8 U3 = Uc ; // U3=-sqrt(3)/2*Ualpha - Ubeta/2   形式与clarker反变换一致
    
    ////不同扇区分别计算
    switch(sector)
    {
        case 1:
        {
            ts[1].id = 4;
            ts[1].ts =(_IQ8mpy(K,U2) >> 8);
            ts[2].id = 6;
            ts[2].ts =(_IQ8mpy(K,U1) >> 8);
            break;
        }
        case 2:
        {
            ts[1].id = 2;
            ts[1].ts = _IQ8mpy(-K,U2) >> 8;
            ts[2].id = 6;
            ts[2].ts = _IQ8mpy(-K,U3) >> 8;
            break;
        }
        case 3:
        {
            ts[1].id = 2;
            ts[1].ts = (_IQ8mpy(K,U1)) >> 8;
            ts[2].id = 3;
            ts[2].ts = (_IQ8mpy(K,U3)) >> 8;
            break;
        }
        case 4:
        {
            ts[1].id = 1;
            ts[1].ts = (_IQ8mpy(-K,U1)) >> 8;
            ts[2].id = 3;
            ts[2].ts = (_IQ8mpy(-K,U2)) >> 8;
            break;
        }
        case 5:
        {
            ts[1].id = 1;
            ts[1].ts = (_IQ8mpy(K,U3)) >> 8;
            ts[2].id = 5;
            ts[2].ts = (_IQ8mpy(K,U2)) >> 8;
            break;
        }
        case 6:
        {
            ts[1].id = 4;
            ts[1].ts = (_IQ8mpy(-K,U3) >> 8);
            ts[2].id = 5;
            ts[2].ts = (_IQ8mpy(-K,U1) >> 8);
            break;
        }
        default:
        return -1;
    }
    ts[0].id = 0;
    if (ts[1].ts < 0) 
    {
        ts[1].ts = 0;
    }
    if (ts[2].ts < 0) 
    {
        ts[2].ts = 0;
    }
    // 防止过调制现象
    if ((ts[1].ts + ts[2].ts) > PWM_DEFAULT_ARR)
    {
        ts[1].ts = _IQ8mpy(_IQ8div(_IQ8(ts[1].ts), _IQ8(ts[1].ts + ts[2].ts)), _IQ8(PWM_DEFAULT_ARR)) >> 8;
        ts[2].ts = _IQ8mpy(_IQ8div(_IQ8(ts[2].ts), _IQ8(ts[1].ts + ts[2].ts)), _IQ8(PWM_DEFAULT_ARR)) >> 8;
    }
    ts[0].ts = PWM_DEFAULT_ARR - ts[1].ts - ts[2].ts;

    //转化为占空比
    int pwmA=0,pwmB=0,pwmC=0;
    for(int i = 1; i < 3; i++)
    {
        if (ts[i].id & (0x1 << 0))
        {
            pwmC += ts[i].ts;
        }
        if (ts[i].id & (0x1 << 1))
        {
            pwmB += ts[i].ts;
        }
        if (ts[i].id & (0x1 << 2))
        {
            pwmA += ts[i].ts;
        }
    }
    pwmC += ts[0].ts / 2;
    pwmB += ts[0].ts / 2;
    pwmA += ts[0].ts / 2;

    //设置pwm
    pFoc->pwm->pwmA = pwmA;
    pFoc->pwm->pwmB = pwmB;
    pFoc->pwm->pwmC = pwmC;
    pFoc->pwm->frameCount++;
    pFoc->pwm->set3PhasePwm(pFoc->pwm, pwmA, pwmB, pwmC);

    // printMat[0] = (float)(pwmA);
    // printMat[1] = (float)(pwmB);
    // printMat[2] = (float)(pwmC);
    return 0;
}
#else

static Int foc_setPhaseVoltage(foc_ *pFoc, _iq8 Ud, _iq8 Uq, _iq8 theta)
{
    _iq8 Ua,Ub,Uc;
    _iq8 pwma,pwmb,pwmc;
    _iq8     max, min, ave, vOffset = 0;

    foc_reversePark(Ud,Uq, theta, pFoc);
    foc_reverseClarke(pFoc->clac_Ualpha, pFoc->clac_Ubeta, pFoc);
    
    vOffset = _IQ8mpy(_IQ8(MOTOR_UDC), _IQ8(0.5));
    Ua = pFoc->calc_Ua + vOffset;
    Ub = pFoc->calc_Ub + vOffset;
    Uc = pFoc->calc_Uc + vOffset;

    max = MAX(Ua, Ub);
    max = MAX(max, Uc);

    min = MIN(Ua, Ub);
    min = MIN(min, Uc);

    ave = _IQ8mpy(max + min, _IQ8(0.5));

    Ua -= ave;
    Ub -= ave;
    Uc -= ave;
    Ua += vOffset;
    Ub += vOffset;
    Uc += vOffset;

    pwma = _IQ8div(Ua,_IQ8(MOTOR_UDC));
    pwma = _IQ8mpy(pwma,_IQ8(PWM_DEFAULT_ARR));
    pwma >>= 8;
    pwmb = _IQ8div(Ub,_IQ8(MOTOR_UDC));
    pwmb = _IQ8mpy(pwmb,_IQ8(PWM_DEFAULT_ARR));
    pwmb >>= 8;
    pwmc = _IQ8div(Uc,_IQ8(MOTOR_UDC));
    pwmc = _IQ8mpy(pwmc,_IQ8(PWM_DEFAULT_ARR));
    pwmc >>= 8;
    pFoc->pwm->set3PhasePwm(pFoc->pwm, pwma, pwmb, pwmc);

    // printMat[0] = (float)(Ua);
    // printMat[1] = (float)(Ub);
    // printMat[2] = (float)(Uc);

    return 0;
}

#endif

static _iq foc_getPos(foc_ *pFoc, _iq Valpha, _iq Vbeta, _iq Ialpha, _iq Ibeta)
{
    static _iq lastIalpha = 0;
    static _iq lastIbeta = 0;
    _iq theta;
    _iq Ealpha;
    _iq Ebeta;

    Ealpha = Valpha - _IQ8mpy(Ialpha,22) - _IQ8mpy((Ialpha - lastIalpha), 15);
    Ebeta = Vbeta - _IQ8mpy(Ibeta,22) - _IQ8mpy((Ibeta - lastIbeta), 15);
    lastIalpha = Ialpha;
    lastIbeta = Ibeta;

    // theta = _IQ8atan2PU(-Ealpha, Ebeta);
    // theta = _IQ8mpy(theta, _IQ8(6.28));
    // printMat[0] = (float)(theta);

    pFoc->pll->pll(pFoc->pll, -(Ealpha << 12), Ebeta << 12);
    theta = pFoc->pll->theta >> 12;

    pFoc->param.Theta = theta;
    // printMat[1] = (float)(theta);
    // printMat[2] = (float)(Ealpha);
    return theta;
}

/*********************************************************************
 * @fn      foc_curruntLoop
 *
 * @brief   实现电流环，
 *
 * @return  FUNC_SOK，成功  FUNC_FAIL，失败
 *          
 */
static Int foc_curruntLoop(struct foc *pFoc)
{
    _iq Ud;
    _iq Uq;
    _iq theta;
    static _iq localId = 0;
    static _iq localIq = 0;
    static _iq openLoopTheta = 0;
    focParam_ *pParam = &pFoc->param;

    pParam->Theta = foc_getPos(pFoc, pFoc->clac_Ualpha, pFoc->clac_Ubeta, pParam->Ialpha, pParam->Ibeta);
    // pParam->Theta += 823;
    pParam->Theta += 402;
    pFoc->IalphaIbetaToIdIq(pFoc);
    localId = _IQ8mpy(pParam->Id,76) + _IQ8mpy(localId,180);
    localIq = _IQ8mpy(pParam->Iq,76) + _IQ8mpy(localIq,180);
    
    if (OPEN_LOOP != pFoc->status)
    {
        Ud = (pFoc->pidId->calc( pFoc->pidId, (pParam->Id_w - localId)));
        Uq = (pFoc->pidIq->calc( pFoc->pidIq, (pParam->Iq_w - localIq)<<2));
        if (Ud > UD_UQ_LIMIT)
        {
            Ud = UD_UQ_LIMIT;
        }
        else if (Ud < -UD_UQ_LIMIT)
        {
            Ud = -UD_UQ_LIMIT;
        }
        if (Uq > UD_UQ_LIMIT)
        {
            Uq = UD_UQ_LIMIT;
        }
        else if (Uq < -UD_UQ_LIMIT)
        {
            Uq = -UD_UQ_LIMIT;
        }
        theta = pFoc->param.Theta;
    }
    else
    {
        Ud = pParam->Ud_w;
        Uq = pParam->Uq_w;
        printMat[0] = (float)(openLoopTheta);
        openLoopTheta += pParam->Omega_w;
        if (openLoopTheta > 1608)
        {
            openLoopTheta -= 1608;
        }
        else if (openLoopTheta < 0)
        {
            openLoopTheta += 1608;
        }
        theta = openLoopTheta;
    }
#if CHOSE_SECTOR
    foc_reversePark(Ud,Uq,theta,pFoc);
    foc_reverseClarke(pFoc->clac_Ualpha, pFoc->clac_Ubeta, pFoc);
    foc_calcSector(pFoc->calc_Ua,pFoc->calc_Ub,pFoc->calc_Uc,pFoc);
    foc_svpwm(pFoc->calc_Ua,pFoc->calc_Ub,pFoc->calc_Uc,pFoc->calc_sector,pFoc);
#else
    foc_setPhaseVoltage(pFoc, Ud, Uq, theta);
#endif

    // printMat[0] = (float)(pParam->Id_w);
    printMat[1] = (float)(localId);
    // printMat[2] = (float)(Ud);

    printMat[0] = (float)(pParam->Theta);
    printMat[2] = (float)(localIq);
    // printMat[2] = (float)(Uq);
    // printMat[3] = (float)(localId);

    
    // printMat[1] = (float)(pParam->Theta);
    // printMat[2] = (float)(pFoc->smo->getOmega(pFoc->smo));
    // // printMat[5] = (float)(Uq);
    printData(printMat,3);
    return 0;
}

/*********************************************************************
 * @fn      focSetUdUq
 *
 * @brief   设置期望的ud uq
 *
 * @return  0,成功， -1,失败
 *          
 */
static Int focSetUdUq(struct foc * pFoc, _iq ud_w, _iq uq_w)
{
    pFoc->param.Ud_w = ud_w;
    pFoc->param.Uq_w = uq_w;
    return 0;
}

/*********************************************************************
 * @fn      focSetIdIq
 *
 * @brief   设置期望的id iq
 *
 * @return  0,成功， -1,失败
 *          
 */
static Int focSetIdIq(struct foc * pFoc, _iq Id_w, _iq Iq_w)
{
    pFoc->param.Id_w = Id_w;
    pFoc->param.Iq_w = Iq_w;
    return 0;
}


/*********************************************************************
 * @fn      focSetIq
 *
 * @brief   设置期望的iq
 *
 * @return  0,成功， -1,失败
 *          
 */
static Int focSetIq(struct foc * pFoc, _iq Iq_w)
{
    pFoc->param.Iq_w = Iq_w;
    return 0;
}

/*********************************************************************
 * @fn      focSetUdUq
 *
 * @brief   设置期望的ud uq
 *
 * @return  0,成功， -1,失败
 *          
 */
static Int focSetOmega(struct foc * pFoc, _iq omega_w)
{
    pFoc->param.Omega_w = omega_w;
    return 0;
}

/*********************************************************************
 * @fn      focSetUdUq
 *
 * @brief   设置期望的ud uq
 *
 * @return  0,成功， -1,失败
 *          
 */
static Int focSetStatus(struct foc * pFoc, focStatus_ sta)
{
    if (sta >= FOC_STATUS_NUM)
    {
        return -1;
    }
    pFoc->status = sta;
    return 0;
}

/*********************************************************************
 * @fn      focCreate
 *
 * @brief   foc构造函数，
 *              void
 *
 * @return  FUNC_SOK，成功  FUNC_FAIL，失败
 *          
 */
foc_ * focCreate(void)
{
    foc_ *pFoc = NULL;

    pFoc = (foc_ *)malloc(sizeof(foc_));
    if (!pFoc)
    {
        return NULL;
    }
    memset((void *)pFoc, 0, sizeof(foc_));

    pFoc->tim = creatFocTim();
    pFoc->pwm = creatPwm();
    pFoc->sampling = createSampling();
    pFoc->pll = pllCreate();
    pFoc->pidId = pidCreate(10, 0, _IQ12(0));
    pFoc->pidIq = pidCreate(2000, 1, 0);

    createClarkeMat(&pFoc->clarkeMat);

    pgFoc = pFoc;

    pFoc->setUdUq = focSetUdUq;
    pFoc->setOmega = focSetOmega;
    pFoc->IaIbIcToIalphaIbeta = foc_IaIbIcToIalphaIbeta;
    pFoc->IalphaIbetaToIdIq = foc_IalphaIbetaToIdIq;
    pFoc->setStatus = focSetStatus;
    pFoc->curruntLoop = foc_curruntLoop;
    pFoc->setIdIq = focSetIdIq;
    pFoc->setIq = focSetIq;

    pFoc->tim->startTim();
    return pFoc;
}


/*********************************************************************
 * @fn      foc_timIrq
 *
 * @brief   重实现定时器中断处理
 *
 * @return  void
 *          
 */
void foc_timIrq(void)
{
    foc_ *pFoc = pgFoc;
    uInt32 tic = 0,toc = 0;
    sampling_    *pSampling = pFoc->sampling;

    /* 清中断 */
    pFoc->tim->clearIrq();

    tic = *( ( volatile uint32_t * ) 0xe000e018 );

    /* 电流采样 */
    pSampling->doSampling(pSampling);

    /* 采样电流clarke变换 */
    pFoc->IaIbIcToIalphaIbeta(pFoc);

    /* 电流环 */
    pFoc->curruntLoop(pFoc);

    toc = *( ( volatile uint32_t * ) 0xe000e018 );
    toc = tic - toc;
}





